#pragma once

#include "vector.h"
#include "smatrix.h"
#include <algorithm>
#include <functional>
#include <cassert>
#include <limits>
//for IO operations #include<glib/g3/io.h>

namespace g3 {

//lower triangular matrix:
//[xx  0  0]
//[yx yy  0]
//[zx zy zz]
template <class T>
class basic_lmatrix;

typedef basic_lmatrix<double> lmatrix;
typedef basic_lmatrix<float> lmatrixf;
typedef basic_lmatrix<long double> lmatrixl;

template <class T>
class basic_lmatrix {
public:
    T xx, yx, zx, yy, zy, zz;
public:
	typedef T * iterator;
	typedef const T * const_iterator;
public:
	T * begin()
		{ return &xx; }
	const T * begin() const
		{ return &xx; }
	T * end()
		{ return &zz + 1; }
	const T * end() const
		{ return &zz + 1; }
	static int size() 
		{ return 6; }
public:
	basic_lmatrix()
		{ }
	basic_lmatrix(T _xx, T _yx, T _zx, T _yy, T _zy, T _zz) :
		xx(_xx), yx(_yx), zx(_zx), yy(_yy), zy(_zy), zz(_zz)
		{ }
	template <class U> 
	basic_lmatrix(const basic_lmatrix<U> & v) :
		xx(T(v.xx)), yx(T(v.yx)), zx(T(v.zx)), yy(T(v.yy)), zy(T(v.zy)), zz(T(v.zz))
		{ }
	basic_lmatrix & identity(T diagval = 1) 
		{ xx = yy = zz = diagval; yx = zx = zy = 0; return * this; }
	basic_lmatrix & zero() 
		{ std::fill(begin(), end(), T(0)); return * this; } 
public:
	basic_vector<T> operator ()(const basic_vector<T> & v) const 
		{ return basic_vector<T>(xx * v.x, yx * v.x + yy * v.y, zx * v.x + zy * v.y + zz * v.z); }
public:
	T det() const
		{ return xx * yy * zz; };
	operator bool() const ///< true if not degenerate
		{ return det() != 0; };
	T tr() const ///< trace of the matrix
		{ return xx + yy + zz; }
public:
    basic_lmatrix & operator += (const basic_lmatrix<T> & v)
		{ std::transform(begin(), end(), v.begin(), begin(), std::plus<T>()); return * this; }
    basic_lmatrix & operator += (T a) ///< adds identity matrix multiplied on a
		{ xx += a; yy += a; zz += a; return * this; }
    basic_lmatrix & operator -= (const basic_lmatrix<T> & v)
		{ std::transform(begin(), end(), v.begin(), begin(), std::minus<T>()); return * this; }
    basic_lmatrix & operator -= (T a)
		{ xx -= a; yy -= a; zz -= a; return * this; }
    basic_lmatrix & operator *= (T a)
		{ std::transform(begin(), end(), begin(), std::bind2nd(std::multiplies<T>(), a)); return * this; }
    basic_lmatrix & operator /= (T a)
		{ std::transform(begin(), end(), begin(), std::bind2nd(std::divides<T>(), a)); return * this; }
	bool operator == (const basic_lmatrix<T> & v) const
		{ return std::equal(begin(), end(), v.begin()); }
	bool operator != (const basic_lmatrix<T> & v) const
		{ return !(*this == v); }
};

template <class T>
inline basic_lmatrix<T> operator + (const basic_lmatrix<T> & a, const basic_lmatrix<T> & b) {
	basic_lmatrix<T> t(a);
    return t += b;
}

template <class T>
inline basic_lmatrix<T> operator + (const basic_lmatrix<T> & v, T a) {
	basic_lmatrix<T> t(v);
    return t += a;
}

template <class T>
inline basic_lmatrix<T> operator - (const basic_lmatrix<T> & a, const basic_lmatrix<T> & b) {
	basic_lmatrix<T> t(a);
    return t -= b;
}

template <class T>
inline basic_lmatrix<T> operator - (const basic_lmatrix<T> & v, T a) {
	basic_lmatrix<T> t(v);
    return t -= a;
}

//unary minus
template <class T>
inline basic_lmatrix<T> operator - (const basic_lmatrix<T> & a) {
	return basic_lmatrix<T>(-a.xx, -a.yx, -a.zx, -a.yy, -a.zy, -a.zz);
}

template <class T>
inline basic_lmatrix<T> operator * (const basic_lmatrix<T> & v, T a) {
	basic_lmatrix<T> t(v);
    return t *= a;
}

template <class T>
inline basic_lmatrix<T> operator * (T a, const basic_lmatrix<T> & v) {
	basic_lmatrix<T> t(v);
    return t *= a;
}

template <class T>
inline basic_vector<T> operator * (const basic_lmatrix<T> & m, const basic_vector<T> & v) {
    return m(v);
}

template <class T>
inline basic_lmatrix<T> operator / (const basic_lmatrix<T> & v, T a) {
	basic_lmatrix<T> t(v);
    return t /= a;
}

///matrix square = l * l^T
template <class T>
inline basic_smatrix<T> msquare(const basic_lmatrix<T> & l) {
	return basic_smatrix<T>(
		l.xx * l.xx, //xx
		l.xx * l.yx, //xy
		l.xx * l.zx, //xz
		l.yx * l.yx + l.yy * l.yy, //yy
		l.yx * l.zx + l.yy * l.zy, //yz
		l.zx * l.zx + l.zy * l.zy + l.zz * l.zz //zz
	);
}

/// solves system of linear equations with lower diagonal matrix: l * x = b, returns x 
template <class T>
basic_vector<T> sle(const basic_lmatrix<T> & l, const basic_vector<T> & b) {
	basic_vector<T> x;
	x.x = b.x / l.xx;
	x.y = (b.y - l.yx * x.x) / l.yy;
	x.z = (b.z - l.zx * x.x - l.zy * x.y) / l.zz;
	return x;
}

/// this version avoids dividing on zero
template <class T>
basic_vector<T> sle0(const basic_lmatrix<T> & l, const basic_vector<T> & b) {
	basic_vector<T> x;
	if (fabs(l.xx) < std::numeric_limits<T>::epsilon())
		x.x = 0;
	else
		x.x = b.x / l.xx;
	if (fabs(l.yy) < std::numeric_limits<T>::epsilon())
		x.y = 0;
	else
		x.y = (b.y - l.yx * x.x) / l.yy;
	if (fabs(l.zz) < std::numeric_limits<T>::epsilon())
		x.z = 0;
	else
		x.z = (b.z - l.zx * x.x - l.zy * x.y) / l.zz;
	return x;
}

/// solves system of linear equations with upper diagonal matrix: l^T * x = b, returns x 
template <class T>
basic_vector<T> sleT(const basic_lmatrix<T> & l, const basic_vector<T> & b) {
	basic_vector<T> x;
	x.z = b.z / l.zz;
	x.y = (b.y - l.zy * x.z) / l.yy;
	x.x = (b.x - l.yx * x.y - l.zx * x.z) / l.xx;
	return x;
}

template <class T>
basic_vector<T> sleT0(const basic_lmatrix<T> & l, const basic_vector<T> & b) {
	basic_vector<T> x;
	if (fabs(l.zz) < std::numeric_limits<T>::epsilon())
		x.z = 0;
	else
		x.z = b.z / l.zz;
	if (fabs(l.yy) < std::numeric_limits<T>::epsilon())
		x.y = 0;
	else
		x.y = (b.y - l.zy * x.z) / l.yy;
	if (fabs(l.xx) < std::numeric_limits<T>::epsilon())
		x.x = 0;
	else
		x.x = (b.x - l.yx * x.y - l.zx * x.z) / l.xx;
	return x;
}

/// Cholesky Decomposition: a = l * l^t, valid only for positive defined matrices
template <class T>
basic_lmatrix<T> cholesky(const basic_smatrix<T> & a) {
	assert (a.positive_defined());
	basic_lmatrix<T> l;
	l.xx = sqrt(a.xx);
	T rxx = 1 / l.xx;
	l.yx = a.xy * rxx;
	l.zx = a.xz * rxx;
	l.yy = sqrt(a.yy - l.yx * l.yx);
	l.zy = (a.yz - l.yx * l.zx) / l.yy;
	l.zz = sqrt(a.zz - l.zx * l.zx - l.zy * l.zy);
	return l;
}

/// solves system of linear equations with positive defined symmetrical matrix: a * x = b, returns x 
template <class T>
inline basic_vector<T> sle(const basic_smatrix<T> & a, const basic_vector<T> & b) {
	basic_lmatrix<T> l = cholesky(a);
	return sleT(l, sle(l, b));
}

/// Cholesky Decomposition: a = l * l^t, 
/// valid for such nonegative defined matrices a, for which the following conditions satisfied:
/// a.xx = 0 => a.xy = 0 && a.xz = 0,
/// a.yy = 0 => a.xy = 0 && a.yz = 0
template <class T>
basic_lmatrix<T> cholesky0(const basic_smatrix<T> & a) {
	assert (a.nonnegative_defined());
	assert (a.xx > 0 || a.xx == 0 && a.xy == 0 && a.xz == 0);
	assert (a.yy > 0 || a.yy == 0 && a.xy == 0 && a.yz == 0);
	basic_lmatrix<T> l;
	l.xx = sqrt(a.xx);
	if (l.xx < std::numeric_limits<T>::epsilon())
		l.yx = l.zx = 0;
	else {
		T rxx = 1 / l.xx;
		l.yx = a.xy * rxx;
		l.zx = a.xz * rxx;
	}
	l.yy = sqrt(a.yy - l.yx * l.yx);
	if (l.yy < std::numeric_limits<T>::epsilon())
		l.zy = 0;
	else
		l.zy = (a.yz - l.yx * l.zx) / l.yy;
	l.zz = sqrt(a.zz - l.zx * l.zx - l.zy * l.zy);
	return l;
}

/* see better version in "smatrix.h"
template <class T>
inline basic_vector<T> sle0(const basic_smatrix<T> & a, const basic_vector<T> & b) {
	basic_lmatrix<T> l = cholesky0(a);
	return sleT0(l, sle0(l, b));
}
*/

} //namespace g3
